//
// Created by 86156 on 25-7-7.
//

#include "control_driver.h"

#include "motor_driver.h"
#include "pid_driver.h"
#include "pt505_driver.h"
#include "rc_driver.h"

CMD_TYPE TEST_mode;

PID_t PID_ROL_Angle;
PID_t PID_YAW_Angle;
PID_t PID_PIT_Angle;
PID_t PID_View;

PID_t PID_ROL_Speed;
PID_t PID_PIT_Speed;
PID_t PID_YAW_Speed;

PID_t PID_Depth;

FLOAT_Angle Angle_Measure;
FLOAT_Angle AngleRate_Measure;
FLOAT_xyz   Gry_Measure;



Orange_Message Version_Message[VERSION_QUANTITY] = {0};

uint16_t Depth;

uint16_t Coordinate_X = 0;
uint16_t Coordinate_Y = 0;




void Motor_Cal(CMD_TYPE*CMD) {

    Motor1.PIN.PWM=MINTHROTTLE+( (+ MOVE_FORWARD_STEP * CMD->Forward_Speed + MOVE_SIDE_STEP * CMD->Side_Speed)* MOTOR1_OFFSET_RATE - PID_YAW_Angle.totalOut * CMD->PID_YAW_flag );
    Motor2.PIN.PWM=MINTHROTTLE-( (+ MOVE_FORWARD_STEP * CMD->Forward_Speed - MOVE_SIDE_STEP * CMD->Side_Speed)* MOTOR2_OFFSET_RATE+ PID_YAW_Angle.totalOut * CMD->PID_YAW_flag );
    Motor3.PIN.PWM=MINTHROTTLE-( (- MOVE_FORWARD_STEP * CMD->Forward_Speed + MOVE_SIDE_STEP * CMD->Side_Speed)* MOTOR3_OFFSET_RATE+ PID_YAW_Angle.totalOut * CMD->PID_YAW_flag );
    Motor4.PIN.PWM=MINTHROTTLE-( (- MOVE_FORWARD_STEP * CMD->Forward_Speed - MOVE_SIDE_STEP * CMD->Side_Speed)* MOTOR4_OFFSET_RATE- PID_YAW_Angle.totalOut * CMD->PID_YAW_flag );
    Motor5.PIN.PWM=MINTHROTTLE+( + (PID_Depth.totalOut + DEPTHOFFSET*MOTOR5_OFFSET_RATE + CMD->Depth_Speed) + PID_ROL_Angle.totalOut -PID_PIT_Angle.totalOut);
    Motor6.PIN.PWM=MINTHROTTLE-( + (PID_Depth.totalOut + DEPTHOFFSET*MOTOR6_OFFSET_RATE + CMD->Depth_Speed) - PID_ROL_Angle.totalOut -PID_PIT_Angle.totalOut);
    Motor7.PIN.PWM=MINTHROTTLE-( + (PID_Depth.totalOut + DEPTHOFFSET*MOTOR7_OFFSET_RATE + CMD->Depth_Speed) + PID_ROL_Angle.totalOut +PID_PIT_Angle.totalOut);
    Motor8.PIN.PWM=MINTHROTTLE-( + (PID_Depth.totalOut + DEPTHOFFSET*MOTOR8_OFFSET_RATE + CMD->Depth_Speed) - PID_ROL_Angle.totalOut +PID_PIT_Angle.totalOut);
}

void Set_Motor_Speed(Motor_t motor) {
    if (RC_Ctl.Rc_Dog.rc_dog < 1 || RC_MODE_FLAG == 0) {
        __HAL_TIM_SET_COMPARE(motor.PIN.PWM_htim ,motor.PIN.PWM_Channel,motor.PIN.PWM);
    }
    else {
        __HAL_TIM_SET_COMPARE(motor.PIN.PWM_htim ,motor.PIN.PWM_Channel,MINTHROTTLE);
    }
}


void Motor_Pid(CMD_TYPE*CMD) {


    PID_Position_Cal(&PID_ROL_Angle,0,Angle_Measure.rol);
    PID_Position_Cal(&PID_YAW_Angle,CMD->Target_Yaw,Angle_Measure.yaw);
    PID_Position_Cal(&PID_PIT_Angle,0,Angle_Measure.pit);


    PID_Position_Cal(&PID_Depth,CMD->Target_Depth,DepthMeasure); //深度环

    // if(PID_Depth.Output>0)
    //     PID_Depth.Output+=60; //深度环电机动力补偿
    //
    // PID_Position_Cal(&PID_View,0,Version_Message[0].x); //视觉环



}
